#include <Servo.h>
#include <Wire.h>
#include "SDuFAT.h"

//adresses
int HMC6352Address = 0x42;
int slaveAddress;
#define GPM 0x68 // GPM I2C Address                                        // Global variable
#define MEM_PW 8 //patte d'alimentation SD

//constantes de réglage
int errcapadm=10;
int neutreservo=90;
int courseservo=90;
int nbrectrlcap=10;
int tempslissage=100;
int nbrepointslissage=20;
int coefflargroute=5;
long largroutemin=20;
long largroutemax=50000;
float pi=3.141592654;
long rmoyterre=6371030;
int AngleInit=15;//angle de servo pour l'initialisation
int tempsinit=1000;//fréquence de prélèvement des caps pour l'initialisation
int nbval=5;//nbr de valeur pour lesquelles on considère que le navire ne peut plus évoluer
int diffmin=3;//difference d'angle minimale
int tempsmes=5000;//temps d'attente entre l'acquisition du premier cap et le second (histoire que le navire reprenne un peu de vitesse)

//Variables globales
byte Address;                                         // Global variable
float Data;    
char chaine[10];                                    // array to hold the data for the ftoa fuction   
int posservo=90;
int capboussoleasuivre;
boolean correctderiveactive=false;
long ecartref;
int WPlatdegres[10];
float WPlatminutes[10];
char WPlatletter[10];
int WPlongdegres[10];
float WPlongminutes[10];
char WPlongletter[10];
long WPvalidate[10];
float targetWPlat;
float targetWPlong;
float previousWPlat;
float previousWPlong;
int targetWP=0;
long targetWPvalidate;

//Objets
Servo servo;  // create servo object to control a servo
 
void setup()
{
  servo.attach(2);  // attaches the servo on pin 2 to the servo object
  //alimentation boussole:
  pinMode(16, OUTPUT);
  digitalWrite(16, LOW);
  pinMode(17, OUTPUT);
  digitalWrite(17, HIGH);
  slaveAddress = HMC6352Address >> 1;   // This results in 0x21 as the address to pass to TWI
  Wire.begin();
  // on my MicroSD Module the power comes from a digital pin
  // I activate it at all times
  pinMode(MEM_PW, OUTPUT);
  digitalWrite(MEM_PW, HIGH);
  
  Serial.begin(9600);
  
  //Liste des waypoints:
  //WP1:
  WPlatdegres[0]=48;
  WPlatminutes[0]=25.135;
  WPlatletter[0]='N';
  WPlongdegres[0]=4;
  WPlongminutes[0]=28.441;
  WPlongletter[0]='O';
  WPvalidate[0]=15;
  //WP2:
  WPlatdegres[1]=48;
  WPlatminutes[1]=25.082;
  WPlatletter[1]='N';
  WPlongdegres[1]=4;
  WPlongminutes[1]=28.432;
  WPlongletter[1]='O';
  WPvalidate[1]=15;
}

int GetDouble(){				      // Get double register value from GPM

  int Value = 0; 
  byte H_Byte = 0;
  byte L_Byte = 0;

  Wire.beginTransmission(GPM);
  Wire.send(Address);				      // Send register start address
  Wire.endTransmission();

  Wire.requestFrom(GPM, 2);			      // Request double register
  while(Wire.available() < 2);			      // Wait for double byte
  H_Byte = Wire.receive();                            // Store one
  L_Byte = Wire.receive();                            // Store two

  Value = (H_Byte * 10) + L_Byte;                     // Adjust for one byte

  return(Value);	                          
}


int GetSingle(){				      // Get single register value from GPM

  int Value = 0; 

  Wire.beginTransmission(GPM);
  Wire.send(Address);				      // Send register start address
  Wire.endTransmission();

  Wire.requestFrom(GPM, 1);			      // Request register
  while(Wire.available() < 1);			      // Wait for single byte
  Value = Wire.receive();                             // and store

  return(Value);	                          
}

float GPSHeading(){ //renvoie le cap en degrés sur un flottant
  float heading;
  Address = 44;                                       // Point to Heading hundreds and tens
  Data = GetDouble();                                 // Read registers from GPM
  heading=10*Data;
  Address = 46;                                       // Point to Heading units
  Data = GetSingle();                                 // Read registers from GPM
  heading=heading+Data;
  Address = 47;                                       // Point to Heading tenths
  Data = GetSingle();                               
  heading=heading+(Data/10);
  return heading;
}

float GPSSpeed(){ //renvoie la vitesse sur un flottant
  float vitesse;
  Address = 52;                                       // Point to Speed hundreds and tens
  Data = GetDouble();                                 // Read registers from GPM
  vitesse=Data*10;
  Address = 54;                                       // Point to Speed units
  Data = GetSingle();                                 // Read registers from GPM
  vitesse=vitesse+Data;
  Address = 55;                                       // Point to Speed units
  Data = GetSingle();                                 // Read registers from GPM
  vitesse=vitesse+(Data/10);
  return vitesse;
}

float GPSLatitude(){ //renvoie la latitude en radians dans notre système (-pi/2 ; pi/2)
  float latitude;
  float minutes;
  char letter;
  Address = 14;                                       // Point to Latitude tens
  Data = GetSingle();                                 // Read registers from GPM
  latitude= Data*10;
  Address = 15;                                       // Point to Latitude units
  Data = GetSingle();                                 // Read registers from GPM
  latitude= latitude+Data;
  Address = 16;                                       // Point to Latitude minutes tens
  Data = GetSingle();                                 // Read registers from GPM
  minutes= Data*10;
  Address = 17;                                       // Point to Latitude minutes units
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data;
  Address = 18;                                       // Point to Latitude minutes tenths
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data/10;
  Address = 19;                                       // Point to Latitude minutes hundredths
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data/100;
  Address = 20;                                       // Point to Latitude minutes thousandths
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data/1000;
  Address = 21;                                       // Point to Latitude minutes ten thousandths
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data/10000;
  latitude=latitude+(minutes/60);
  Address = 22;                                       // Point to Latitude character register
  Data = GetSingle();                                 // Read registers from GPM
  letter=Data;
  if (letter=='S'){                 // Prise en conpte de la lettre et passage dans notre système
    latitude=-latitude;
  }
  return radians(latitude);
}

float GPSLongitude(){  //renvoie la longitude en radians dans notre système (-pi ; pi)
  float longitude;
  float minutes;
  char letter;
  Address = 23;                                       // Point to Longitude hundreds
  Data = GetSingle();                                 // Read registers from GPM
  longitude= Data*100;
  Address = 24;                                       // Point to Longitude tens
  Data = GetSingle();                                 // Read registers from GPM
  longitude= longitude+Data*10;
  Address = 25;                                       // Point to Longitude units
  Data = GetSingle();                                 // Read registers from GPM
  longitude= longitude+Data;
  Address = 26;                                       // Point to Longitude minutes tens
  Data = GetSingle();                                 // Read registers from GPM
  minutes= Data*10;
  Address = 27;                                       // Point to Longitude minutes units
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data;
  Address = 28;                                       // Point to Longitude minutes tenths
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data/10;
  Address = 29;                                       // Point to Longitude minutes hundredths
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data/100;
  Address = 30;                                       // Point to Longitude minutes thousandths
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data/1000;
  Address = 31;                                       // Point to Longitude minutes ten thousandths
  Data = GetSingle();                                 // Read registers from GPM
  minutes= minutes+Data/10000;
  longitude=longitude+(minutes/60);
  Address = 32;                                       // Point to Longitude character register
  Data = GetSingle();                                 // Read registers from GPM
  letter=Data;
  if (letter=='E'){                                    // Prise en conpte de la lettre et passage dans notre système
    longitude=-longitude;
  }
  return radians(longitude);
}


int Boussole()
{
  byte headingData[2];
  int i, headingValue;
  Wire.beginTransmission(slaveAddress);
  Wire.send("A");              // The "Get Data" command
  Wire.endTransmission();
  delay(10);                   // The HMC6352 needs at least a 70us (microsecond) delay
  // after this command.  Using 10ms just makes it safe
  // Read the 2 heading bytes, MSB first
  // The resulting 16bit word is the compass heading in 10th's of a degree
  // For example: a heading of 1345 would be 134.5 degrees
  Wire.requestFrom(slaveAddress, 2);        // Request the 2 byte heading (MSB comes first)
  i = 0;
  while(Wire.available() && i < 2)
  { 
    headingData[i] = Wire.receive();
    i++;
  }
  headingValue = headingData[0]*256 + headingData[1];  // Put the MSB and LSB together
  headingValue = headingValue/ 10; //cap sur 360°
  return headingValue;
} 


int BoussoleLissee(){
  
  int somme=0;
  int t[nbrepointslissage];
  
  for (int i=0; i<nbrepointslissage; i++){//boucle pendant laquelle on prélève un cap tous les tempslissage; on prélève nbrepointslissage valeurs.
    t[i]=Boussole();
    delay(tempslissage);
    //Serial.println(t[i]);
   }
  for (int i=0; i<nbrepointslissage; i++){//somme de tous les caps prélevés ci-dessu
    somme=somme+t[i];
   } 
  double valeurmoyenne = somme/nbrepointslissage;//moyenne
  int v = (int) valeurmoyenne;//transformation en entier
  //Serial.print("valmoy : ");
  //Serial.println(v);
  return v;
 }


char *ftoa(char *a, double f, int precision) {
  long p[] = {0,10,100,1000,10000,100000,1000000,10000000,100000000};
  
  char *ret = a;
  long heiltal = (long)f;
  itoa(heiltal, a, 10);
  while (*a != '\0') a++;
  *a++ = '.';
  long desimal = abs((long)((f - heiltal) * p[precision]));
  itoa(desimal, a, 10);
  return ret;
} 

long dist(float latitudedep, float longitudedep, float latitudear, float longitudear){ // Retourne la distance en mètres entre 2 points GPS avec une erreur maximale de 0.2%
  float latitude;
  float longitude;
  float a;
  float c;
  float d;
  float e;
  long distance;
  latitude= (abs(latitudedep-latitudear));
  longitude= min(min(abs(longitudedep-longitudear), abs((2*pi+longitudedep)-longitudear)), abs(longitudedep-(longitudear+2*pi)));
  a = (square(sin(latitude/2.0))) + cos(latitudedep)*cos(latitudear)*(square(sin(longitude/2.0)));
  d=sqrt(a);
  e=sqrt(1-a);
  c = 2*atan2(d, e);
  distance= rmoyterre*c;
  return distance;
}

float GPSHeadingTh(float latitudedep, float longitudedep, float latitudear, float longitudear){  // Retourne le cap théorique en degrés pour relier 2 points GPS
  float heading;
  float a;
  float b;
  float c;
  float d;
  float longitude;
  longitude= min(min(abs(longitudedep-longitudear), abs((2*pi+longitudedep)-longitudear)), abs(longitudedep-(longitudear+2*pi)));
  a=sin(longitude)*cos(latitudear);
  c=cos(latitudedep)*sin(latitudear);
  d=sin(latitudedep)*cos(latitudear)*cos(longitude);
  b=c-d;
  heading=atan2(a,b);
  heading=degrees(heading);
  if (sin(longitudear-longitudedep)>0){                    //passage de [-180°; 180°] à [0°; 360°] pour rejoindre la convention des caps d'un compas.
   heading=360-abs(heading); 
  }
  return heading;
}

float ConversionLatitude(int degres, float minutes, char letter){
  float latitude;
  latitude= degres+ minutes/60;
  if (letter=='S'){                                      // Prise en conpte de la lettre et passage dans notre système
    latitude=-latitude;
  }
  return radians(latitude);
}

float ConversionLongitude(int degres, float minutes, char letter){
  float longitude;
  longitude= degres+ minutes/60;
  if (letter=='E'){                                    // Prise en conpte de la lettre et passage dans notre système
    longitude=-longitude;
  }
  return radians(longitude);
}

boolean isWPOK(){
 if (dist( GPSLatitude(), GPSLongitude(), targetWPlat, targetWPlong)<=targetWPvalidate){
   return true;
 }
}

boolean SuiviCap(int cap){
int errcap;
int correrr=10;
int capboussole;
for(int i=0; i<nbrectrlcap; i++){    //boucle de correction du cap
//calcul de l'erreur de cap
capboussole= BoussoleLissee();
  if(capboussole<cap){
    if(cap-capboussole<((360+capboussole)-cap)){
      errcap=cap-capboussole;
    }
    else{
      errcap=(360+capboussole)-cap;
    }
  }
  else{
    if(capboussole-cap<((360+cap)-capboussole)){
      errcap=capboussole-cap;
    }
    else{
      errcap=(360+cap)-capboussole;
    }
  }
  //Si le cap est correct
  if(errcap<errcapadm){
  //    Serial.println("Cap correct");
  SD.print("log.txt","\nCap correct");
  return true;
  }
  else{
  // Calcul de l'angle de correction:
  correrr=errcap/10;
  //incrémenter ou décrémenter l'angle du safran
  if(cap>180){
     if (capboussole>(cap-180) && capboussole<cap){
       //virer sur babord
       if (posservo+correrr<neutreservo+courseservo){
         posservo=posservo+correrr;
       }
       else{
        posservo=neutreservo+courseservo; 
       }
       servo.write(posservo);
   }
     else{
       //virer sur tribord
       if (posservo-correrr>neutreservo-courseservo){
         posservo=posservo-correrr;
       }
       else{
        posservo=neutreservo-courseservo; 
       }
       servo.write(posservo);
   }
  }
  else{
     if (capboussole<cap || capboussole>cap+180){
       //virer sur babord
       if (posservo+correrr<neutreservo+courseservo){
         posservo=posservo+correrr;
       }
       else{
       posservo=neutreservo+courseservo; 
       }
       servo.write(posservo);
   }
     else{
       //virer sur tribord
       if (posservo-correrr>neutreservo-courseservo){
         posservo=posservo-correrr;
       }
       else{
        posservo=neutreservo-courseservo; 
       }
       servo.write(posservo);
   }
  }
  }
//  Serial.print("Capboussole ");
//  Serial.println(capboussole);
//  Serial.print("Posservo ");
//  Serial.println(posservo);
//  Serial.print("Erreur ");
//  Serial.println(errcap);
    SD.print("log.txt","\nSuivi cap: ");
    SD.print("log.txt","\nCapboussole: ");
    SD.println("log.txt", ftoa(chaine, capboussole, 2));
    SD.print("log.txt","\nPoservo: ");
    SD.println("log.txt", ftoa(chaine, posservo, 2));
    SD.print("log.txt","\nErreur: ");
    SD.println("log.txt", ftoa(chaine, errcap, 2));
}
return false;
}

boolean SuiviRoute(float latitudedep, float longitudedep, float latitudear, float longitudear){     // cap de depart boussole a definir au debut
//calcul des variables nécessaires pour la fonction:
long longroute= dist(latitudedep, longitudedep, latitudear, longitudear);
float headingroute=GPSHeadingTh(latitudedep, longitudedep, latitudear, longitudear);
float theoricalheading=GPSHeadingTh( GPSLatitude(), GPSLongitude(), latitudear, longitudear);
float currentheading=GPSHeading();
float headingtocurrentpoint=GPSHeadingTh(latitudedep, longitudedep, GPSLatitude(), GPSLongitude());
float distancedone=dist(latitudedep, longitudedep, GPSLatitude(), GPSLongitude());
long largroute= longroute/coefflargroute;
long ecart;
 //Calcul de la largeur de la route
 if (largroute>largroutemax){
   largroute=largroutemax;
 }
  if (largroute<largroutemin){
   largroute=largroutemin;
 }
 //Vérification que le bateau est bien présent à l'intérieur du couloir de la route définie, calcul de l'écart à la route:
 ecart=asin(sin(distancedone/rmoyterre)*sin(radians(headingtocurrentpoint-headingroute)))*rmoyterre; //négatif si position actuelle à gauche de la route.
 if(abs(ecart)<largroute/2){                                                              //bateau présent dans le couloir          
   if((abs(ecart)<largroute/3) && correctderiveactive==false){
     ecartref=ecart;
    if(currentheading<theoricalheading){                                                //correction de l'erreur de cap
      if(theoricalheading-currentheading<(360+currentheading)-theoricalheading){
        if(capboussoleasuivre+(theoricalheading-currentheading)<360){
          capboussoleasuivre=capboussoleasuivre+(theoricalheading-currentheading);
        }
        else{
          capboussoleasuivre=capboussoleasuivre+(theoricalheading-currentheading)-360;
        }
      }
      else{
        if(capboussoleasuivre+((360+currentheading)-theoricalheading)<360){
          capboussoleasuivre=capboussoleasuivre+((360+currentheading)-theoricalheading);
        }
        else{
          capboussoleasuivre=capboussoleasuivre+((360+currentheading)-theoricalheading)-360;
        }
      }
    }
    else{
      if(currentheading-theoricalheading<((360+theoricalheading)-currentheading)){    
        if(capboussoleasuivre-(currentheading-theoricalheading)>0){
          capboussoleasuivre=capboussoleasuivre-(currentheading-theoricalheading);
        }
        else{
          capboussoleasuivre=360+(capboussoleasuivre-(currentheading-theoricalheading));
        }
      }
      else{
        if(capboussoleasuivre-((360+theoricalheading)-currentheading)>0){
          capboussoleasuivre=capboussoleasuivre-((360+theoricalheading)-currentheading);
        }
        else{
          capboussoleasuivre=360+(capboussoleasuivre-((360+theoricalheading)-currentheading));
        }
      }
    }   
   }
   else{// on est en dehors de la route de largeur 2/3 largroute ou bien la correction est active
     correctderiveactive=true;
   Serial.println("Correction dérive active");
     if (abs(ecart)<largroute/6){ //dérive corrigée
       correctderiveactive=false;
     }
     else{ // correction de la dérive
       if (ecart<ecartref){
          ecartref=ecart; 
       }
       else{
         if(ecart>0){  //virer sur babord
           if(capboussoleasuivre+1<360){
             capboussoleasuivre++;
           }
           else{
             capboussoleasuivre=capboussoleasuivre-359;
           }
         }
         else{  //virer sur tribord
           if(capboussoleasuivre-1>0){
             capboussoleasuivre--;
           }
           else{
             capboussoleasuivre=capboussoleasuivre+359;
           }
         }
       }
     }   
   }  
   Serial.println("Ecart route: ");
   Serial.print(ecart);
   Serial.println("Cap boussole à suivre: ");
   Serial.print(capboussoleasuivre);
    SD.print("log.txt","\nSuivi route: ");
    SD.print("log.txt","\nEcart route: ");
    SD.println("log.txt", ftoa(chaine, ecart, 2));
    SD.print("log.txt","\nNouveau cap boussole: ");
    SD.println("log.txt", ftoa(chaine, capboussoleasuivre, 2));
  return true;  
 }
 Serial.println("Impossible");
 return false;
}

int capimmobile(){ //fonction de détection d'immobilisation du navire
  int a=0; //cap 
  int d=10000; //nombre de cases du tableau
  boolean cond=true;
  int t[d]; //tableau
  int diffcap[d];
  int f=0;
  
  Serial.println("rien");

  t[0]=0;
  //boucle pendant laquelle on prélève un cap tous les tempsinit;
    for(int i=1;cond==true;i++){
    t[i]=Boussole();
    delay(tempsinit);
    //Methode permettant d'obtenir la difference d'angle entre t[i] et t[i+1] et la stocker dans diffcap[i]
      //capboussole=t[i], cap=t[i+1], errcap=diffcap[i]
    if(t[i-1]<t[i]){
    if(t[i]-t[i-1]<((360+t[i-1])-t[i])){
    diffcap[i]=t[i]-t[i-1];}
    else{
    diffcap[i]=(360+t[i-1])-t[i];}}
    else{
    if(t[i-1]-t[i]<((360+t[i])-t[i-1])){
    diffcap[i]=t[i-1]-t[i];}
    else{
    diffcap[i]=(360+t[i])-t[i-1];}}
    Serial.print("diff cap : ");
    Serial.println(diffcap[i]);  
    
    if (diffcap[i] <= diffmin){
         f++;  
          Serial.print("f : ");
          Serial.println(f); 
          }
    //Si sur nbval valeurs, abs(t[i+1]-t[i]) est inferieur à une valeur minimale diffmin, on considère que le bateau n'évolue plus  
         
      else{f=0;}; //sinon rien ne se passe, la boucle principale (en i) continue
                            
         
        
     if(f>=nbval){
       cond=false; //la boucle principale s'arrete alors
       a=GPSHeading(); //alors on fixe a au cap GPS
        //SD.print("log.txt","\nMethode Initialisation");
        //SD.println("log.txt","\nCap1: ");
        //SD.print("log.txt", ftoa(chaine,a, 2));
        servo.write(neutreservo); //et le servo se met à 0 
        f=0;//reinitialisation de f   
        }  
       else{} //sinon rien ne se passe, la boucle continue, f est reinitialise
      }
      //Serial.print("cap : ");
      //Serial.println(a);
return a;  
 }
    
int initialisation(){ 
  int c=0; //cap vent
  servo.write(neutreservo+AngleInit); //on fait tourner le bateau avec un angle de barre faible AngleInit dans le sens des aiguilles d'une montre
  delay(tempsmes);//on attend un moment que le navire prenne de la vitesse
  int a=capimmobile();
   Serial.print("CAP1 : ");
   Serial.println(a); 
  servo.write(neutreservo-AngleInit); //le servo se met dans la position inverse  
  delay(tempsmes);//on attend un moment que le navire reprenne de la vitesse 
  int b=capimmobile();
   Serial.print("CAP2 : ");
   Serial.println(b); 
  servo.write(neutreservo);
  //calcul final de la direction du vent
  int l=0;
  if(a<b){
    if(b-a<((360+a)-b)){
    l=b-a;}
    else{
    l=(360+a)-b;}}
    else{
    if(a-b<((360+b)-a)){
    l=a-b;}
    else{
    l=(360+b)-a;}}
  c=a+l/2; //on calcule  l'angle moyen et ça nous donne la direction du vent
  //SD.println("log.txt","\nCapVent: ");
  //SD.print("log.txt", ftoa(chaine,c, 2));
       Serial.print("CAP VENT : ");
        Serial.println(c); 
  return c;
}

void loop(){
//  boolean test;                                         //test correction et contrôle de cap
//  test=SuiviCap(50);
//  if(test==true){
//    //Serial.println("Cap traite");
//    SD.print("log.txt","\nCap traite: ");
//  }
//  else{
//    //Serial.println("Impossible de corriger le cap");
//    SD.print("log.txt","\nImpossible de corriger le cap");
//  }
//  Serial.print("Heading: ");                             //test cap GPS
//  Serial.println(GPSHeading());
//  Serial.print("Speed: ");                               //test vitesse GPS
//  Serial.println(GPSSpeed());
//  Serial.print("100000*longitude: ");                    // test longitude GPS
//  Serial.println(100000*GPSLongitude());
//  Serial.print("100000*latitude: ");                     // test latitude GPS
//  Serial.println(100000*GPSLatitude());
//  Serial.println(1000*ConversionLongitude(45, 25.2356, 'E')); //Test conversion coordonnées GPS
//  Serial.println(1000*ConversionLatitude(45, 25.2356, 'N'));
//  Serial.println(dist(ConversionLatitude(48, 22.666, 'N'), ConversionLongitude(4, 29.628, 'O'), ConversionLatitude(48, 18.701, 'N'), ConversionLongitude(4, 29.947, 'O'))); //test distance
//  Serial.println(GPSHeadingTh(ConversionLatitude(48, 11.706, 'N'), ConversionLongitude(4, 53.990, 'O'), ConversionLatitude(43, 34.831, 'N'), ConversionLongitude(8, 20.909, 'O'))); // test cap théorique
SuiviRoute(ConversionLatitude(48, 25.135, 'N'), ConversionLongitude(4, 28.441, 'O'), ConversionLatitude(48, 25.082, 'N'), ConversionLongitude(4, 28.432, 'O'));
delay(3000);
}
